//变力用"wall_z", 0.073(压水杯测试得) k_stiffness", 1000.0 配合plane_adrc0和fgravity_compensation3实验
#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/robot_state.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <Eigen/Geometry>
#include <cstdio>

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("ee_force_sim_100hz");

  // 参数：末端 link、墙位置、刚度、力上限
  std::string ee_link = node->declare_parameter<std::string>("ee_link", "end");
  double wall_z       = node->declare_parameter<double>("wall_z", 0.073);   // 平面位置 位置越低，实际位置越低，实际力越大，（越晚碰到即接触时间滞后）
  double k_stiffness  = node->declare_parameter<double>("k_stiffness", 850.0); // N/m 刚度越小，实际位置越低，实际力越大
  double fz_limit     = node->declare_parameter<double>("fz_limit", 30.0);    // N

  // PSM（需要 robot_description 已加载）
  auto tf_buffer   = std::make_shared<tf2_ros::Buffer>(node->get_clock());
  auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
  auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node, "robot_description", tf_buffer, "psm");

  if (!psm->getPlanningScene())
  {
    RCLCPP_FATAL(node->get_logger(), "Planning scene not configured（robot_description 未加载？）");
    return 1;
  }

  psm->startStateMonitor("/joint_states");
  psm->startSceneMonitor();
  rclcpp::sleep_for(std::chrono::milliseconds(300));

  if (!psm->getRobotModel()->hasLinkModel(ee_link))
  {
    RCLCPP_FATAL(node->get_logger(), "Link '%s' 不存在，请检查 URDF/SRDF。", ee_link.c_str());
    return 1;
  }

  auto pub = node->create_publisher<geometry_msgs::msg::WrenchStamped>("/sim_force", 10);

  // 100 Hz
  auto timer = node->create_wall_timer(std::chrono::milliseconds(10), [=]() {
    // 读取当前末端位姿
    planning_scene_monitor::LockedPlanningSceneRO scene(psm);
    const auto& state = scene->getCurrentState();
    const Eigen::Isometry3d& T = state.getGlobalLinkTransform(ee_link);
    const double z = T.translation().z();

    // 力模型：z<=wall_z -> 0；z>wall_z -> Fz = k*(z-wall_z)，限幅
    double Fx = 0.0, Fy = 0.0, Fz = 0.0;
    if (z < wall_z)
    {
      Fz = k_stiffness * (wall_z - z);
      if (Fz > fz_limit) Fz = fz_limit;
    }

    // 发布 WrenchStamped
    geometry_msgs::msg::WrenchStamped msg;
    msg.header.stamp = node->now();
    msg.header.frame_id = "base_link";  // 可按需改
    msg.wrench.force.x = Fx;
    msg.wrench.force.y = Fy;
    msg.wrench.force.z = Fz;
    // 不模拟力矩：默认为 0
    pub->publish(msg);

    // 打印（带时间戳，避免日志开销，使用 printf）
    const double t = rclcpp::Time(msg.header.stamp).seconds();
    std::printf("[%.6f] z=%.4f  Fx=%.2f  Fy=%.2f  Fz=%.2f\n", t, z, Fx, Fy, Fz);
    std::fflush(stdout);
  });

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
